/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8 noet:
 * $Id: fixedpoint.cpp,v 1.1 2004/09/12 03:40:39 poine Exp $
 *
 * (c) 2003 Trammell Hudson <hudson@rotomotion.com>
 */

#include <cstddef>
#include <inttypes.h>

class fixedpoint
{
private:
	/*
	 * Constructor to make a fixedpoint value in place.  It
	 * has a bogus second argument to distinguish it from the
	 * integer constructor.  Not for public use!
	 */
	fixedpoint(
		const int		value,
		int			/* Bogus */
	) :
		value			( value )
	{
	}


	/*
	 * Use 24 bits of the 32 for the right side of the decimal
	 * point.  This gives us 0.00000005 per bit, which should
	 * be sufficient given the limited precision of the
	 * sensor sets.
	 *
	 * Valid ranges are +/- 256.0 for the numbers that can
	 * be represented.  No out of range errors are thrown,
	 * so be careful.
	 */
	static const int32_t		mantissa_bits	= 24;
	static const int32_t		base		= 1 << 24;

public:

	/*
	 * Initialize a fixed point value frmo a double.
	 */
	fixedpoint(
		const double		d
	) :
		value			( int32_t( d * base ) )
	{
	}
	

	/*
	 * Initialize a fixed point value from an int.  This is
	 * fairly cheap.
	 */
	fixedpoint(
		const int		i
	) :
		value			( i << mantissa_bits )
	{
	}

	/*
	 * Default value is zero.  Luckily zero is represented as
	 * an actual zero.
	 */
	fixedpoint()
	:
		value			( 0 )
	{
	}


	/*
	 * Addition and subtraction are easy.
	 * No overflow is detected.
	 */
	const fixedpoint
	operator + (
		const fixedpoint		b
	) const
	{
		return fixedpoint( b.value + this->value, 0 );
	}

	const fixedpoint
	operator += (
		const fixedpoint		b
	)
	{
		this->value += b.value;
		return *this;
	}


	const fixedpoint
	operator - (
		const fixedpoint		b
	) const
	{
		return fixedpoint( b.value - this->value, 0 );
	}


	const fixedpoint
	operator -= (
		const fixedpoint		b
	)
	{
		this->value -= b.value;
		return *this;
	}


	/*
	 * Multiplication is harder.  We promote to 64 bits long
	 * and do the 64 bit multiply to preserve any extra data.
	 * The shift afterwards is to clear out the extra base
	 * that results from (a*B)*(b*B) = (a*b)*B^2.
	 *
	 * The 64 bit multiply could be simplified, since it is
	 * actually a 64 bit result of two 32 bit ints.
	 */
	const fixedpoint
	operator * (
		const fixedpoint		b
	) const
	{
		const int64_t		res
			= int64_t(this->value) * int64_t(b.value);

		return fixedpoint(
			res >> this->mantissa_bits,
			0
		);
	}


	/*
	 * Division is also tricky.  If we did just a/b, we would
	 * round to nearest integers thanks to the integer math.
	 * Instead we promote to 64 bits long, which guarantees
	 * that the numerator is larger than the largest possible
	 * denominator.  This also works well, because we would have
	 * to multiply by the base anyway.
	 */
	const fixedpoint
	operator / (
		const fixedpoint		b
	) const
	{
		const int64_t		v
			= int64_t(this->value) << this->mantissa_bits;

		return fixedpoint( v / b.value, 0 );
	}


	/*
	 * Scalar multiplication and division are easy, however.
	 * We might overflow on the multiplication, but we will never
	 * know...
	 */
	const fixedpoint
	operator * (
		const int32_t			i
	) const
	{
		return fixedpoint( this->value * i, 0 );
	}

	const fixedpoint
	operator *= (
		const int32_t			i
	)
	{
		this->value *= i;
		return *this;
	}

	const fixedpoint
	operator / (
		const int32_t			i
	) const
	{
		return fixedpoint( this->value / i, 0 );
	}

	const fixedpoint
	operator /= (
		const int32_t			i
	)
	{
		this->value /= i;
		return *this;
	}


	/*
	 * Convert back to a double for IO or trig operations.
	 * This is expensive -- it has an integer -> double cast
	 * and a double division.
	 */
	operator double () const
	{
		return double(this->value) / this->base;
	}


	/*
	 * This should be private.  It is the actual 32 bits that
	 * represent our fixed point value.
	 */
	int32_t			value;
};


#include <iostream>
using namespace std;

int
main( void )
{
	fixedpoint		a( 3.14159 );
	fixedpoint		b( 8 );
	fixedpoint		c;

	cout << "sizeof(fixedpoint a)=" << sizeof(a) << endl;
	cout << "a=" << a.value << " = " << double(a) << endl;
	cout << "b=" << b.value << " = " << double(b) << endl;

	c = a+b;
	cout << "a+b=" << c.value << " = " << double(c) << endl;

	c = a*b;
	cout << "a*b=" << c.value << " = " << double(c) << endl;

	c = a/b;
	cout << "a/b=" << c.value << " = " << double(c) << endl;

	c = a * 8;
	cout << "a*5=" << c.value << " = " << double(c) << endl;

	c = a / 8;
	cout << "a/5=" << c.value << " = " << double(c) << endl;

	a += b;
	cout << "a+=b" << a.value << " = " << double(a) << endl;

	a -= b;
	cout << "a-=b" << a.value << " = " << double(a) << endl;

	a *= 8;
	cout << "a*=9" << a.value << " = " << double(a) << endl;

	a /= 8;
	cout << "a/=9" << a.value << " = " << double(a) << endl;

}

